/**************************************************************************************************
* Title: SmurfsPhysicsManager.cpp
* Authors: Gael Huber, Erin Mooney
* Description: Manages updating of physics
**************************************************************************************************/
#include "SmurfsPhysicsManager.h"

// Singleton instance
template<> PhysicsManager* Singleton<PhysicsManager>::singleton = 0;

/**************************************************************************************************
* Constructor
**************************************************************************************************/
PhysicsManager::PhysicsManager(void) {
	// Do nothing
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
PhysicsManager::~PhysicsManager(void) {
	// Do nothing
}

/**************************************************************************************************
* Initialize physics
**************************************************************************************************/
void PhysicsManager::startUp(void) {
	timer = new Timer();
	timer->setupTimer();
	timer->setStartTime();
	timer->getDeltaTime();
}

/**************************************************************************************************
* Shutdown
**************************************************************************************************/
void PhysicsManager::shutDown(void) {

}

/**************************************************************************************************
* Get the singleton instance
**************************************************************************************************/
PhysicsManager& PhysicsManager::getSingleton(void) { 
	assert(singleton);
	return *singleton;
}

/**************************************************************************************************
* Get the singleton pointer
**************************************************************************************************/
PhysicsManager* PhysicsManager::getSingletonPtr(void) {
	return singleton;
}

/**************************************************************************************************
* Update the scene
**************************************************************************************************/
void PhysicsManager::update(void) {
	// Get dt
	float dt = timer->getDeltaTime();

	// Find the torques and applied forces
	//****************************************************************************
	// Find R
	Vector3 r = bar->halfWidths;

	// Calculate gravitational force being applied to the bar
	Vector3 gravitationalForce = Vector3(0.0f, GRAVITY, 0.0f) * bar->mass;// * dt;
	Vector3 gravitationalTorque = r.cross(gravitationalForce);
	gravitationalForce -= gravitationalTorque;

	// Reverse r for other forces
	r = bar->halfWidths * 2.0f;

	// Calculate the spring force being applied to the bar
	Vector3 springForce = spring->getForce();// * dt;
	Vector3 springTorque = r.cross(springForce);
	springForce -= springTorque;

	// Externally applied force
	//bar->appliedForce *= dt;
	if(bar->appliedForce.magnitude() > 0)
		int a = 0;
	Vector3 appliedTorque = r.cross(bar->appliedForce);
	bar->appliedForce -= appliedTorque;

	// Sumation of torques
	Vector3 totalTorque = gravitationalTorque + springTorque + appliedTorque;

	// Update angular momentum
	//****************************************************************************
	bar->angularMomentum.x += totalTorque.x * dt;
	bar->angularMomentum.y += totalTorque.y * dt;
	bar->angularMomentum.z += totalTorque.z * dt;
	
	// Damping and friction
	bar->angularMomentum *= spring->dampingConstant;

	// Find the inverse of the moment of inertia tensor
	//****************************************************************************
	Matrix invI = bar->inertiaTensor.getInverse();

	// Find the matrix equivalent of the current rotation quaternion
	//****************************************************************************
	Matrix q = bar->axes.toMatrix();

	// Find q transpose
	//****************************************************************************
	Matrix qT = q.getTranspose();

	// Find the final matrix representative of moment of inertia
	//****************************************************************************
	Matrix moi = (q * invI) * qT;
	
	// Update angular velocity
	//****************************************************************************
	Quaternion rotW = Quaternion(0.0f, 0.0f, 0.0f, 0.0f);
	rotW.x = (bar->angularMomentum.x * moi.matrix[0][0]) + 
		(bar->angularMomentum.y * moi.matrix[0][1]) + 
		(bar->angularMomentum.z * moi.matrix[0][2]);
	rotW.y = (bar->angularMomentum.x * moi.matrix[1][0]) + 
		(bar->angularMomentum.y * moi.matrix[1][1]) + 
		(bar->angularMomentum.z * moi.matrix[1][2]);
	rotW.z = (bar->angularMomentum.x * moi.matrix[2][0]) + 
		(bar->angularMomentum.y * moi.matrix[2][1]) + 
		(bar->angularMomentum.z * moi.matrix[2][2]);

	bar->aVelocity.x = rotW.x;
	bar->aVelocity.y = rotW.y;
	bar->aVelocity.z = rotW.z;

	// Calculate the quaternion by which to rotate
	//****************************************************************************
	bar->dQ.w = bar->axes.w + dt * ((rotW.w * bar->axes.w) - (rotW.x * bar->axes.x) - (rotW.y * bar->axes.y) - (rotW.z * bar->axes.z)) / 2.0f;
	bar->dQ.x = bar->axes.x + dt * ((rotW.w * bar->axes.x) + (rotW.x * bar->axes.w) + (rotW.y * bar->axes.z) - (rotW.z * bar->axes.y)) / 2.0f;
	bar->dQ.y = bar->axes.y + dt * ((rotW.w * bar->axes.y) - (rotW.x * bar->axes.z) + (rotW.y * bar->axes.w) + (rotW.z * bar->axes.x)) / 2.0f;
	bar->dQ.z = bar->axes.z + dt * ((rotW.w * bar->axes.z) + (rotW.x * bar->axes.y) - (rotW.y * bar->axes.x) + (rotW.z * bar->axes.w)) / 2.0f;
	bar->dQ.normalize();	// Thank you Stuart

	// Update position of spring
	Vector3 dimensions = bar->halfWidths * 2.0f;
	dimensions.y = 0.0f; dimensions.z = 0.0f;
	dimensions.rotate(bar->dQ);
	spring->updatePositionGlobal(dimensions + bar->tPosition);

	// Ensure that the applied force is limited to one keypress
	bar->appliedForce = Vector3();
}

/**************************************************************************************************
* Getters and setters
**************************************************************************************************/
RigidBody* PhysicsManager::getBar(void) { return bar; }
void PhysicsManager::setBar(RigidBody* value) { bar = value; }

Spring* PhysicsManager::getSpring(void) { return spring; }
void PhysicsManager::setSpring(Spring* value) { spring = value; }

Vector3 PhysicsManager::getPivot(void) { return pivot; }
void PhysicsManager::setPivot(Vector3 value) { pivot = value; }
